#include <cassert>
#include <sstream>
#include "Gps.h"

namespace uav
{
namespace nav
{

using namespace math;

void Gps::nextSentence()
{
	// Get a complete sentence (starts with $, ends with \n) from the data stream
	std::string sentence;
	do {
		sentence = stream.readLine();
	} while (sentence[0] != '$');

	// Remove the $ at the beginning and the \r\n at the end
	sentence = sentence.substr(1, sentence.length() - 3);

	fields.clear();

	// Parse the sentence into fields
	std::string::size_type i = 0, j = sentence.find(',');
	while (j != std::string::npos) {
		fields.push_back(sentence.substr(i, j - i));
		i = j + 1;
		j = sentence.find(',', i);
	}
	fields.push_back(sentence.substr(i));

	// Remove the checksum from the last field (if there)
	const std::string &last = fields.back();
	fields.back() = last.substr(0, last.find('*'));

	// Determine the sentence type
	const std::string &first = fields.front();
	if (first == "GPGGA")
		type = SENTENCE_GPGGA;
	else if (first == "PGRMV")
		type = SENTENCE_PGRMV;
	else
		type = SENTENCE_UNKNOWN;

	// Check for fix quality
	if (type == SENTENCE_GPGGA) {
		assert(fields.size() > 6);
		if (fields[6] == "1")
			quality = FIX_QUALITY_BASIC;
		else if (fields[6] == "2")
			quality = FIX_QUALITY_DIFFERENTIAL;
		else if (fields[6] == "6")
			quality = FIX_QUALITY_ESTIMATED;
		else
			quality = FIX_QUALITY_NONE;
	}
}

GeoCoord Gps::getPosition() const
{
	assert(type == SENTENCE_GPGGA);
	assert(quality != FIX_QUALITY_NONE);
	assert(fields.size() > 9);

	Fixed lat, lon, alt;
	int deg;

	// Get latitude
	const std::string& latField = fields[2];
	std::istringstream latDegInput(latField.substr(0, 2));
	latDegInput >> deg;
	lat = degreesMinutesToRadians(deg, Fixed(latField.substr(2)));
	if (fields[3] == "S")
		lat = -lat;

	// Get longitude
	const std::string& lonField = fields[4];
	std::istringstream lonDegInput(lonField.substr(0, 3));
	lonDegInput >> deg;
	lon = degreesMinutesToRadians(deg, Fixed(lonField.substr(3)));
	if (fields[5] == "W")
		lon = -lon;

	// Get altitude
	alt = Fixed(fields[9]);

	return GeoCoord(lat, lon, alt);
}

Vector3 Gps::getVelocity() const
{
	assert(type == SENTENCE_PGRMV);
	assert(quality != FIX_QUALITY_NONE);
	assert(fields.size() > 3);

	return Vector3(Fixed(fields[1]), Fixed(fields[2]), Fixed(fields[3]));
}

}}
